#include "MapNode.h"
#include <fstream>
#define ROBOT_HALF 10
using std::ofstream;
using std::string;
using std::endl;
using std::min;
using std::max;
using std::cout;
static const char WALL = 'w';
static const char FREE_SPACE = 'f';
static const char UNKNOWN = 'u';


void save_map(string path)
{
  std::ofstream myfile (path.c_str());
  if (myfile.is_open())
  {
    for (int x = 0; x < ROWS; x++ )
    {
      for (int y = 0; y < COLS; y++ )
      {
        bool placed = false;
        for (int i = max(x-ROBOT_HALF,0); i < min(x+ROBOT_HALF,ROWS) && !placed; i++ )
        {
            for (int j = max(y-ROBOT_HALF,0); j < min(y+ROBOT_HALF,COLS) && !placed; j++ )
            {
              if (mat[i][j] == MAP_WALL)
              {
                myfile << WALL;
                placed = true;
              }    
            }
        }
        if( !placed)
        {
          if (mat[x][y] == MAP_FREE)
          {
            myfile << FREE_SPACE; 
          } 
          else
          {
            myfile << UNKNOWN; 
          }
        }        
      }
      myfile << endl;
    }
    myfile.close();
  }
}

int main(int argc, char **argv)
{

	ros::init(argc, argv, "saveMap");
	ros::NodeHandle n;
	ros::Subscriber state_sub = n.subscribe("State/Info", 1, state_update);
	ros::Subscriber enc_sub = n.subscribe("/serial/encoder", 1, recive_encoder);//when "/encoder" topic is revieved call recive_encoder function
	ros::Subscriber ir_sub = n.subscribe("ir_info", 1, recieve_ir);
	vis_pub = n.advertise<visualization_msgs::Marker>("/visualization_marker", 1 );
	map_pub = n.advertise<nav_msgs::OccupancyGrid>("/grid",1);
	rob_pos_pub = n.advertise<visualization_msgs::Marker>("/visualization_pos",1);
	pos_pub = n.advertise<saint_nr_9::RobotPosition>("/position", 1);
	ros::Rate loop_rate(100);
  
  mat = new map_type*[ROWS];
  for(int i = 0; i < ROWS; i++) {
    mat[i] = new map_type[COLS];
    memset(mat[i], MAP_UNEXPLORED, COLS);
  }
  for (int r = 0; r < ROWS; r++) {
	  delete [] mat[r];
	}
	delete [] mat; 
}
